

#include "HeaderFiles.h"
#include "uart_bluetooth.h"

STR_BLUETOOTH_FRAME bluetoothCmdx1;
STR_BLUETOOTH_FRAME bluetoothCmdx2;

STR_BLUETOOTH_FRAME *bluetoothCmd[2] = {&bluetoothCmdx1, &bluetoothCmdx2};


int8_t  debugcmd_forwardcnt = 0;
int8_t  debugcmd_backwardcnt = 0;
int8_t  debugcmd_rightcnt = 0;
int8_t  debugcmd_leftcnt = 0;

uint16_t  bluetooth_comm_time = 0;


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void uart_bluetooth_analysis(STR_BLUETOOTH_FRAME *frame[], STR_RINGBUF* bufhdl)
{

	static uint8_t  databuf[256];
	uint8_t length;
	static uint8_t num = 0;

	bluetooth_comm_time++;
	
	length = ringBufRead(bufhdl, (char*)databuf, 256);
	
	for(uint16_t i=0;i<length;i++)
	{
		if((databuf[i] == BLUETOOTH_CMD_HEAD) && (databuf[i+18] == BLUETOOTH_CMD_TAIL))
		{
			memcpy(&frame[num]->data[0], &databuf[i], BLUETOOTH_CMD_LENGTH);
			num++;
			num %= 2;
			
			bluetooth_comm_time = 0;
		}
	}
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void debug_cmd_analysis(void)
{
	if(bluetooth_comm_time == 0)
	{
		if((bluetoothCmdx1.data[1] == '0') && (bluetoothCmdx2.data[1] == '0'))
		{
			debugcmd_forwardcnt = 0;
			debugcmd_leftcnt = 0;	

			userHdl.ctrl.speedline_mps = 0;
			userHdl.ctrl.speedangle_dps = 0;
			
		}
		
		else if((bluetoothCmdx1.data[1] == '1') || (bluetoothCmdx2.data[1] == '1'))
		{
			debugcmd_forwardcnt++;
			
			userHdl.ctrl.speedline_mps += 0.1f;
		
		}
		
		else if((bluetoothCmdx1.data[1] == '2') || (bluetoothCmdx2.data[1] == '2'))
		{
			debugcmd_forwardcnt--;
			
			userHdl.ctrl.speedline_mps -= 0.1f;
		
		}
		
		else if((bluetoothCmdx1.data[1] == '3') || (bluetoothCmdx2.data[1] == '3'))
		{
			debugcmd_leftcnt++;
			userHdl.ctrl.speedangle_dps += 10.0f;
		
		
		}

		else if((bluetoothCmdx1.data[1] == '4') || (bluetoothCmdx2.data[1] == '4'))
		{
			debugcmd_leftcnt--;
			userHdl.ctrl.speedangle_dps -= 10.0f;
		
		}
	}
	
	else if(bluetooth_comm_time > 2000)
	{
		debugcmd_forwardcnt = 0;
		debugcmd_leftcnt = 0;	
		
		userHdl.ctrl.speedline_mps = 0;
		userHdl.ctrl.speedangle_dps = 0;
	}
	
	
}







